#include "mpc.h"
#include <iostream>
#include <memory>
using namespace LinearMpc;
int main()
{

	{ //(uint state_dim,uint control_dim, uint distur_dim)
		//Mpc::Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, uint mpc_windows, uint max_xu_constraint, uint max_final_constraint)
		int state_dim = 4;
		int control_dim = 3;
		int distur_dim = 2;
		int mpc_windows = 5;
		int max_xu_constraint = 2;
		int max_final_constraint = 2;
		std::shared_ptr<DiscreteLinearSys> dis_sys_ptr(new DiscreteLinearSys(state_dim, control_dim, distur_dim));

		Eigen::MatrixXd state_matrix(state_dim, state_dim);
		state_matrix << 1.1, 1.2, 1.3, 1.4,
			1.5, 1.6, 1.7, 1.8,
			2.1, 2.2, 2.3, 2.4,
			2.5, 2.6, 2.7, 2.8;

		dis_sys_ptr->set_state_matrix(state_matrix);
		std::cout << "state matrix is:" << std::endl;
		std::cout << state_matrix << std::endl;
		Eigen::MatrixXd control_matrix(state_dim, control_dim);
		control_matrix << 3.1, 3.2, 3.3,
			3.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "control matrix is:" << std::endl;
		std::cout << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);

		Eigen::MatrixXd distur_matrix(state_dim, distur_dim);
		distur_matrix << 0.1, 0.2,
			0.2, 0.3,
			0.4, 0.5,
			0.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "distur_matrix is:" << std::endl;
		std::cout << distur_matrix << std::endl;
		//Mpc(std::shared_ptr<DiscreteLinearSys> dis_sys_ptr, unsigned int mpc_windows,
		//unsigned int max_xu_constraint, unsigned int max_delta_x, unsigned int max_delta_u, unsigned int max_final_constraint);
		Mpc test_mpc(dis_sys_ptr, mpc_windows, max_xu_constraint, 1, 1, max_final_constraint);

		//test defualt hessian
		Eigen::MatrixXd temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		if (temp_matrix ==
			Eigen::MatrixXd::Identity(state_dim * (mpc_windows + 1) + control_dim * mpc_windows,
									  state_dim * (mpc_windows + 1) + control_dim * mpc_windows))
			std::cout << "defualt hessian set is ok:" << std::endl;
		std::cout << temp_matrix << std::endl;

		//test defualt gradient
		if (test_mpc.gradient() == Eigen::VectorXd::Zero(state_dim * (mpc_windows + 1) + control_dim * mpc_windows))
		{
			std::cout << "defualt gradient is ok:" << std::endl;
		}
		std::cout << test_mpc.gradient().transpose() << std::endl;
		//test defualt constraint matrix

		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "defualt constriant_matrix is ok:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << "default bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;

		//add constraint
		/*int Mpc::set_xu_affine(const std::vector<double> x_index, const std::vector<double> x_factor,
						   const std::vector<double> u_index, const std::vector<double> u_factor,
						   int constraint_index, double min, double max)*/
		test_mpc.set_xu_affine({0, 2}, {1.0}, {}, {}, 0, 0, 0); //error msg will be apear
																//int set_xu_bound(int constraint_index, double min, double max);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter add error xu_affine is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter add error xu_affine is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		test_mpc.set_xu_affine({0, 5}, {1.0, 2.0}, {}, {}, 0, -1, 1);
		//test_mpc.set_xu_bound()
		test_mpc.set_xu_affine({0, 2}, {1.0, 2.0}, {1, 2}, {1.0, 1.0}, 0, -1, 1);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter add  xu_affine 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter add  xu_affine 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		//int set_xu_bound(int constraint_index, double min, double max);
		test_mpc.set_xu_bound(0, -2, 2);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter reset bound 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter reset bound 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;

		test_mpc.set_xu_affine({1, 3}, {1.0, 2.0}, {0, 1}, {1.0, 1.0}, 1, -3, 3);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter add  xu_affine 1 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter add  xu_affine 1 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		//int set_final_constraint(int constraint_index, const std::vector<double> x_index, const std::vector<double> x_factor,
		//						 double min, double max);
		test_mpc.set_final_constraint(0, {0, 1, 2, 3}, {0.1, 0.2, 0.3, 0.4}, -0.5, 0.5);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter add  final 0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter add  final 0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;

		state_matrix << 0.1, 0.2, 0.3, 1.4,
			0.5, 0.6, 1.7, 1.8,
			0.1, 1.2, 2.3, 2.4,
			0.5, 2.6, 2.7, 2.8;
		;
		std::cout << "reset state matrix is:" << std::endl
				  << state_matrix << std::endl;
		dis_sys_ptr->set_state_matrix(state_matrix);
		control_matrix << 0.1, 0.2, 3.3,
			0.5, 3.6, 3.7,
			4.1, 4.1, 4.3,
			4.5, 4.6, 4.7;
		std::cout << "reset control matrix is:" << std::endl
				  << control_matrix << std::endl;
		dis_sys_ptr->set_control_matrix(control_matrix);
		distur_matrix << 1.1, 0.2,
			1.2, 0.3,
			1.4, 0.5,
			1.6, 0.7;
		dis_sys_ptr->set_distur_matrix(distur_matrix);
		std::cout << "reset distur_matrix is:" << std::endl
				  << distur_matrix << std::endl;
		/*	bool update_mpc(
		const Eigen::MatrixXd &q, const Eigen::MatrixXd &r,
		const Eigen::VectorXd &x0, const Eigen::MatrixXd &x_ref,
		const Eigen::MatrixXd &distur_ref); // dis_sys_ptr  with disturbance*/
		test_mpc.update_mpc(test_mpc.q(), test_mpc.r(), test_mpc.x0(), test_mpc.x_ref(), test_mpc.distur_ref());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter reset dis_sys is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter reset dis_sys is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;
		//set x0
		Eigen::VectorXd x0(4);
		x0 << 1, 1, 1, 1;

		test_mpc.update_mpc(test_mpc.q(), test_mpc.r(), x0, test_mpc.x_ref(), test_mpc.distur_ref());
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter set x0 is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter reset x0 is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;
		//set distur_ref
		Eigen::MatrixXd distur_ref(2, 1);
		distur_ref << 0.1,
			0.2;
		test_mpc.update_mpc(test_mpc.q(), test_mpc.r(), x0, test_mpc.x_ref(), distur_ref);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter set distur_ref is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter reset distur_ref is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;
		//set q r
		Eigen::MatrixXd q(state_dim, state_dim);
		q << 1.1, 0, 0, 0,
			0, 1.2, 0, 0,
			0, 0, 1.3, 0,
			0, 0, 0, 1.4;
		Eigen::MatrixXd r(control_dim, control_dim);
		r << 1.8, 0, 0,
			0, 1.9, 0,
			0, 0, 1.5;

		test_mpc.update_mpc(q, r, x0, test_mpc.x_ref(), distur_ref);
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << "hassian_matrix affter set q r is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " gradient affter reset q r is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		//set x_ref
		Eigen::VectorXd x_ref(4);
		x_ref << 1, 2, 1, 2;
		test_mpc.update_mpc(q, r, x0, x_ref, distur_ref);
		temp_matrix = Eigen::MatrixXd(test_mpc.hessian());
		std::cout << "hassian_matrix affter set x_ref is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " gradient affter reset x_ref is:" << std::endl;
		std::cout << test_mpc.gradient().transpose() << std::endl;
		//set delta_x
		//int set_delta_x(unsigned int constraint_index, unsigned int x_index, double min, double max);
		test_mpc.set_delta_x(6, 1, -100, 100); //error
		test_mpc.set_delta_x(0, 1, -100, 100);
		test_mpc.update_mpc(test_mpc.q(), test_mpc.r(), x0, test_mpc.x_ref(), distur_ref);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter set delta_x is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter set delta_x is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;
		//set delta_x_bound
		test_mpc.set_delta_x_bound(6, 1, -100, 100); //error
		test_mpc.set_delta_x_bound(0, 1, -200, 200);
		std::cout << " bound affter set delta_x bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;
		//set delta_u
		//int set_delta_u(unsigned int constraint_index, unsigned int u_index, double min, double max);
		test_mpc.set_delta_u(5, 1, -100, 100); //error
		test_mpc.set_delta_u(0, 5, -100, 100); //error
		test_mpc.set_delta_u(0, 1, -100, 100);
		test_mpc.update_mpc(test_mpc.q(), test_mpc.r(), x0, test_mpc.x_ref(), distur_ref);
		temp_matrix = Eigen::MatrixXd(test_mpc.constraint_matrix());
		std::cout << "constriant_matrix affter set delta_u is:" << std::endl;
		std::cout << temp_matrix << std::endl;
		std::cout << " bound affter set delta_u is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;
		//set delta_u_bound
		test_mpc.set_delta_u_bound(0, 1, -300, 300);
		std::cout << " bound affter set delta_u bound is:" << std::endl;
		std::cout << test_mpc.lowerBound().transpose() << std::endl;
		std::cout << test_mpc.upperBound().transpose() << std::endl;
		std::cout << test_mpc.lowerBound().rows() << " " << test_mpc.upperBound().rows() << " " << temp_matrix.rows() << std::endl;
		test_mpc.update_mpc(test_mpc.q(), test_mpc.r(), x0, test_mpc.x_ref(), distur_ref);
		test_mpc.mpc_solve();
	}
}